#include "LdPcl.h"

int main()
{
    //建立目标
    pcl::PassThrough<Point> pass;
    pcl::NormalEstimation<Point, pcl::Normal> ne;
    pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg;
    pcl::PCDWriter writer;
    pcl::ExtractIndices<Point> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<Point>::Ptr tree(new pcl::search::KdTree<Point>());

    //建立中间数据
    pcl::PointCloud<Point>::Ptr Zhi_Fil(new pcl::PointCloud<Point>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<Point>::Ptr Plane_Fil(new pcl::PointCloud<Point>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices);

    Pcloud cloud(new Cloud());
    Pcloud Fil_cloud(new Cloud());
    std::string pcd_saved = "/home/milo/Documents/visual/2Champion/Lidars/Livox/Mars/catkin_ws/src/livox_camera_calib/data/6.pcd";
    int result = pcl::io::loadPCDFile<Point>(pcd_saved, *cloud);
    if (result == -1)
    {
        PCL_ERROR("不能显示");
        return -1;
    }

    // std::vector<Point, Eigen::aligned_allocator<Point>>::const_iterator position ;
    // Cloud::iterator it;
    // for(it = cloud->begin(); it != cloud->end();)
    // {
    //     if(it->z < -0.2)
    //         it = cloud->erase(it);
    //     else
    //         it++;
    // }

    // pcl::visualization::CloudViewer Ti("Remove points z < 0");
    // Ti.showCloud(cloud);
    // while (!Ti.wasStopped())
    // {
    // }

    pcl::RadiusOutlierRemoval<Point> pcFilter;
    pcFilter.setInputCloud(cloud);
    pcFilter.setRadiusSearch(0.1);
    pcFilter.setMinNeighborsInRadius(8);
    pcFilter.filter(*Fil_cloud);

    //直通滤波
    Pcloud Zhi2_fil(new Cloud());
    pass.setInputCloud(Fil_cloud);
    pass.setFilterFieldName("x");
    // pass.setFilterLimits(5, 7);
    pass.setFilterLimits(-10, 10);
    // pass.setFilterFieldName("y");
    // pass.setFilterLimits(0, 1);
    pass.filter(*Zhi2_fil);
    Pcloud Z_fil(new Cloud());

    pass.setInputCloud(Zhi2_fil);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(-10, 10);
    pass.filter(*Z_fil);;

    pass.setInputCloud(Z_fil);
    pass.setFilterFieldName("z");
    // pass.setFilterLimits(-0.2, 1);
    pass.setFilterLimits(-1, 2);
    pass.filter(*Zhi_Fil);

    // pcl::visualization::CloudViewer ZHI("Zhitong");
    // ZHI.showCloud(Zhi_Fil);
    // while (!ZHI.wasStopped())
    // {
    // }

    pcl::search::KdTree<Point>::Ptr Ptree(new pcl::search::KdTree<Point>);
    Ptree->setInputCloud(Zhi_Fil);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<Point> ec;
    ec.setClusterTolerance(0.02);  //点间距离，测试得到理想值
    ec.setMinClusterSize(150);
    ec.setMaxClusterSize(2000);
    ec.setSearchMethod(Ptree);
    ec.setInputCloud(Zhi_Fil);
    ec.extract(cluster_indices);

    // int j = 0;
    for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();
        it != cluster_indices.end();
        it++
    )
    {
        Pcloud cloud_cluster(new Cloud());
        for(const auto& idx : it->indices)
            cloud_cluster->push_back((*Zhi_Fil)[idx]);
        
        cloud_cluster->width = cloud_cluster->size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size() << "data points" << std::endl;
    //     pcl::visualization::CloudViewer cluster("cluster");
    //     cluster.showCloud(cloud_cluster);
    //     while(!cluster.wasStopped()){}
    }
    //估计点法线
    ne.setSearchMethod(tree);
    ne.setInputCloud(Zhi_Fil);
    ne.setKSearch(50);
    // ne.setRadiusSearch(0.05);
    ne.compute(*cloud_normals);

    //建立平面分割模型并设置参数
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight(0.3);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(100);
    seg.setDistanceThreshold(0.08);
    seg.setInputCloud(Zhi_Fil);
    seg.setInputNormals(cloud_normals);

    //分割多个模型
    int all_points = (int)Zhi_Fil->size();
    Pcloud Extraction(new Cloud());
    while(Zhi_Fil->size() > 0.3 * all_points)
    {
        seg.setInputCloud(Zhi_Fil);
        seg.setInputNormals(cloud_normals);
        seg.segment(*inliers_plane, *coefficients_plane);
        if(inliers_plane->indices.size() == 0)
            {
                std::cout << "Could not estimate a planar model any more";
                break;
            }
        extract.setInputCloud(Zhi_Fil);
        extract.setIndices(inliers_plane);
        extract.setNegative(false);

        extract.filter(*Extraction);
        // pcl::visualization::CloudViewer Times_Extraction("TExtraction");
        // Times_Extraction.showCloud(Extraction);
        // while(!Times_Extraction.wasStopped()){}

        extract.setNegative(true);
        Pcloud Temp(new Cloud());
        extract.filter(*Temp);
        *Zhi_Fil = *Temp;

        pcl::PointCloud<pcl::Normal>::Ptr Temp_normals (new pcl::PointCloud<pcl::Normal>);
        extract_normals.setNegative(true);
        extract_normals.setInputCloud(cloud_normals);
        extract_normals.setIndices(inliers_plane);
        extract_normals.filter(*Temp_normals);
        *cloud_normals = *Temp_normals;
    }

    // pcl::visualization::CloudViewer After("After all plane extraction");
    // After.showCloud(Zhi_Fil);
    // while(!After.wasStopped()){}

    /*
    //获取平面内点
    seg.segment(*inliers_plane, *coefficients_plane);
    std::cerr << "plane coefficients: " << *coefficients_plane << std::endl;

    //将平面点从原始点云中剔除
    extract.setInputCloud (Zhi_Fil);
    extract.setIndices(inliers_plane);
    extract.setNegative(false);

    //获取平面
    Pcloud cloud_plane(new Cloud());
    extract.filter(*cloud_plane);
    pcl::visualization::CloudViewer Plane_extraction("Plane_extraction");
    Plane_extraction.showCloud(cloud_plane);
    while(!Plane_extraction.wasStopped())
    {
    }

    //将平面点移除，获取剩下的点云
    extract.setNegative(true);
    extract.filter(*Plane_Fil);
    extract_normals.setNegative(true);
    extract_normals.setInputCloud(cloud_normals);
    extract_normals.setIndices(inliers_plane);
    extract_normals.filter(*cloud_normals2);

    pcl::visualization::CloudViewer Plane_out("Without plane");
    Plane_out.showCloud(Plane_Fil);
    while(!Plane_out.wasStopped()){}
    */

    //建立圆柱分割模型并设置参数
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight(0.5);
    seg.setMaxIterations(10000);
    seg.setDistanceThreshold(0.08);
    seg.setRadiusLimits(0, 0.5);
    // seg.setInputCloud(Plane_Fil);
    seg.setInputCloud(Zhi_Fil);
    // seg.setInputNormals(cloud_normals2);
    seg.setInputNormals(cloud_normals);

    //获取圆柱内点和参数
    seg.segment(*inliers_cylinder, *coefficients_cylinder);
    std::cerr << "Cylinder coeffcients: " << *coefficients_cylinder << std::endl;
    //把圆柱分割
    // extract.setInputCloud(Plane_Fil);
    extract.setInputCloud(Zhi_Fil);
    extract.setIndices(inliers_cylinder);
    extract.setNegative(false);
    Pcloud cloud_cylinder(new Cloud());
    extract.filter(*cloud_cylinder);
    if (cloud_cylinder->points.empty())
    {
        std::cerr << "找不到圆柱" << std::endl;
        return 0;
    }

    else
    {
        // pcl::visualization::CloudViewer cylinder_extraction("cylinder_extraction");
        // cylinder_extraction.showCloud(cloud_cylinder);
        // while (!cylinder_extraction.wasStopped())
        // {
        // }
    }
    // pcl::visualization::PointCloudColorHandler<Point> rgb(cloud_cylinder);
    // boost::shared_ptr<pcl::visualization::PCLVisualizer> view(new pcl::visualization::PCLVisualizer("圆柱提取查看"));
    // view->addPointCloud(cloud_cylinder, rgb);

    Point p1, p2, p3;
    p1.x = (*coefficients_cylinder).values[0];
    p1.y = (*coefficients_cylinder).values[1];
    p1.z = (*coefficients_cylinder).values[2];

    // int k = 10;
    // p2.x = p1.x + k*(*coefficients_cylinder).values[3];
    // p2.y = p1.y + k*(*coefficients_cylinder).values[4];
    // p2.z = p1.z + k*(*coefficients_cylinder).values[5];
    for (size_t i = 0; i < 100; i++)
    {
        p2.x = p1.x + i * 0.01 * (*coefficients_cylinder).values[3];
        p2.y = p1.y + i * 0.01 * (*coefficients_cylinder).values[4];
        p2.z = p1.z + i * 0.01 * (*coefficients_cylinder).values[5];

        p3.x = p1.x - i * 0.01 * (*coefficients_cylinder).values[3];
        p3.y = p1.y - i * 0.01 * (*coefficients_cylinder).values[4];
        p3.z = p1.z - i * 0.01 * (*coefficients_cylinder).values[5];
        cloud->points.push_back(p2);
        cloud->points.push_back(p3);
    }
    // pcl::visualization::CloudViewer AddLine("visulable");
    // AddLine.showCloud(cloud);
    // while (!AddLine.wasStopped())
    // {
    // }
    // view->setBackgroundColor(1,1,1);
    // view->addCoordinateSystem(1.0);
    // view->addLine<pcl::PointXYZ>(p1, p2, 255, 0, 0, "line");
    // while(!view->wasStopped())
    // {
    // }
    return 0;
}